function [trajectory, grid_map, stats] = run_grid_slam(true_traj, obstacles, dt, Q, laser_range, laser_fov, num_beams, laser_noise, show_progress)
% 运行占据栅格SLAM + MCL
% 输入:
%   true_traj - 真实轨迹 [3 x steps]
%   obstacles - 障碍物位置 [N x 2]
%   dt - 时间步长
%   Q - 过程噪声协方差
%   laser_range - 激光最大距离
%   laser_fov - 激光视场角
%   num_beams - 激光束数量
%   laser_noise - 测距噪声标准差
%   show_progress - 是否显示进度（默认false）
% 输出:
%   trajectory - 估计轨迹 [3 x steps]
%   grid_map - 占据栅格地图
%   stats - 统计信息

    if nargin < 9
        show_progress = false;
    end
    
    steps = size(true_traj, 2);
    
    % 地图参数
    map_size = 50;
    grid_resolution = 0.2;
    grid_size = round(map_size / grid_resolution);
    
    % MCL参数
    N_particles = 200;
    resample_threshold = 0.5;
    
    % 初始化
    occupancy_map = zeros(grid_size, grid_size);  % log-odds
    particles = initialize_mcl_particles(N_particles, true_traj(:, 1)', [0.5, 0.5, 0.1]);
    trajectory = zeros(3, steps);
    
    % 主循环
    progress_interval = max(1, round(steps/20));
    
    for t = 1:steps
        % 显示进度
        if show_progress && (mod(t, progress_interval) == 0 || t == 1 || t == steps)
            percent = round(100 * t / steps);
            % 统计已建图区域
            prob_map = 1 ./ (1 + exp(-occupancy_map));
            mapped_cells = sum(prob_map(:) > 0.7 | prob_map(:) < 0.3);
            coverage = 100 * mapped_cells / (grid_size * grid_size);
            fprintf('  [%3d%%] 步骤 %4d/%4d | 地图覆盖: %5.1f%%\r', ...
                    percent, t, steps, coverage);
            if t == steps
                fprintf('\n');
            end
        end
        
        % 计算控制输入
        if t == 1
            control = [0; 0];
        else
            dx = true_traj(1, t) - true_traj(1, t-1);
            dy = true_traj(2, t) - true_traj(2, t-1);
            dtheta = wrapToPi(true_traj(3, t) - true_traj(3, t-1));
            control = [sqrt(dx^2 + dy^2) / dt; dtheta / dt];
        end
        
        % MCL预测
        particles = mcl_predict(particles, control, dt, Q);
        
        % 激光扫描（使用真实位姿获取观测）
        scan = simulate_laser_scan(true_traj(:, t), obstacles, laser_range, laser_fov, num_beams, laser_noise);
        
        % MCL更新（使用激光扫描和占据地图）
        particles = mcl_update(particles, scan, occupancy_map, laser_range, laser_fov, num_beams, grid_resolution, map_size);
        
        % 重采样
        N_eff = 1 / sum([particles.weight].^2);
        if N_eff < N_particles * resample_threshold
            particles = resample_mcl_particles(particles);
        end
        
        % 估计状态
        estimated_state = estimate_mcl_state(particles);
        trajectory(:, t) = estimated_state;
        
        % 更新占据栅格地图（使用估计位姿，与其他算法统一）
        occupancy_map = update_occupancy_grid(occupancy_map, estimated_state, scan, ...
                                              laser_range, laser_fov, num_beams, grid_resolution, map_size);
    end
    
    % 构建栅格地图
    grid_map = struct();
    grid_map.occupancy = occupancy_map;
    grid_map.resolution = grid_resolution;
    grid_map.size = map_size;
    grid_map.grid_size = grid_size;
    
    % 计算地图统计
    prob_map = 1 ./ (1 + exp(-occupancy_map));
    occupied_cells = sum(prob_map(:) > 0.7);
    free_cells = sum(prob_map(:) < 0.3);
    mapped_cells = occupied_cells + free_cells;
    
    % 计算统计信息
    position_error = sqrt(sum((true_traj(1:2, :) - trajectory(1:2, :)).^2, 1));
    stats = struct();
    stats.errors = position_error;
    stats.avg_error = mean(position_error);
    stats.max_error = max(position_error);
    stats.mapped_cells = mapped_cells;
    stats.occupied_cells = occupied_cells;
    stats.free_cells = free_cells;
    stats.map_coverage = 100 * mapped_cells / (grid_size * grid_size);
end

